/*
 * @Description: 
 * @Author: Sang Hao
 * @Date: 2021-11-09 09:57:01
 * @LastEditTime: 2021-11-22 21:54:08
 * @LastEditors: Sang Hao
 */

#include <pcl/kdtree/flann.h>
#include <pcl/common/common_headers.h>
#include "glog/logging.h"

#include "lidar_slam/models/extract/plane_ransac_extract.hpp"

namespace lidar_slam {
// bool PlaneRansacExtract::SetParam(int k_search, int max_iters, float normal_weight) {
// 	k_search_ = k_search;
// 	max_iters_ = max_iters;
// 	normal_weight_ = normal_weight;
// 	return true;
// }

// PlaneRansacExtract::PlaneRansacExtract(int k_search, int max_iters, float normal_weight) 
// 	: k_search_(k_search), max_iters_(max_iters), normal_weight_(normal_weight){

// }

PlaneRansacExtract::PlaneRansacExtract(const YAML::Node& config_node) {
	// k_search_ = config_node["k_search"].as<int>();
	max_iters_ = config_node["max_iters"].as<int>();
	// normal_weight_ = config_node["normal_weight"].as<float>();
	distance_thresh_ = config_node["distance_thresh"].as<float>();
	eps_angle_ = config_node["eps_angle"].as<float>();
	model_type_ = config_node["model_type"].as<std::string>();
	plane_num_ = config_node["plane_num"].as<int>();
	axis_  = {	config_node["axis"][0].as<float>(), 
				config_node["axis"][1].as<float>(), 
				config_node["axis"][2].as<float>()	};
}

std::string PlaneRansacExtract::GetModelType() {
	return model_type_;
}

/* 得到模型参数 */
bool PlaneRansacExtract::Extract(const CloudData::CLOUD_PTR& input_cloud_ptr, 
								 pcl::ModelCoefficients::Ptr& model_coeff_ptr, 
								 pcl::PointIndices::Ptr& inliers_indices_ptr) {

	sac_segment_.setMethodType(pcl::SAC_RANSAC);	/* 设置参数估计方法 */
	if (model_type_ == "SACMODEL_PARALLEL_PLANE") {
		/* 平面模型与给定轴线平行 */
		sac_segment_.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
		// std::cout << "设置模型：SACMODEL_PARALLEL_PLANE" << std::endl;
	} else if (model_type_ == "SACMODEL_PERPENDICULAR_PLANE") {
		/* 平面模型和给定轴线垂直 */
		// std::cout << "设置模型：SACMODEL_PERPENDICULAR_PLANE" << std::endl;
		sac_segment_.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
	} else {
		/* 什么都没写，则用普通平面模型 */
		sac_segment_.setModelType(pcl::SACMODEL_PLANE);
	}
	
	sac_segment_.setInputCloud(input_cloud_ptr);
	sac_segment_.setAxis(axis_);
	sac_segment_.setMaxIterations(max_iters_);
	sac_segment_.setDistanceThreshold(distance_thresh_);
	sac_segment_.setEpsAngle(pcl::deg2rad(eps_angle_));
	sac_segment_.setOptimizeCoefficients(true);
	/* 明明有很多点，为何会提示内点不足呢？ */
	std::cout << input_cloud_ptr->points.size() << std::endl;
	sac_segment_.segment(*inliers_indices_ptr, *model_coeff_ptr);
	
	/* 给一个没有提取出来的判断条件 */
	if (inliers_indices_ptr->indices.size() == 0) {	
        LOG(WARNING) << "RANSAC失败！没有提取出满足条件的模型! " 
					 << "此时点云数量为：" << input_cloud_ptr->points.size() 
					 << std::endl << std::endl;
		return false;
	}
	return true;
}
}

